﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Net;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Threading;
using System.IO.Ports;

// Telemetry collection for F1 2011. This test harness collects data from the game 
// whenever you press the Connect button.  This kicks off a thread which continuously
// fetches packets from the game until terminated using the Disconnect button.
//
// To work with F1 2011 you need to change the hardware_settings_config.xml file
// located in My Documents\My Games\FormulaOne2011\hardwaresettings folder.  Change
// the motion enabled line so that it looks like this:
//
// <motion enabled="true" ip="127.0.0.1" port="20777" delay="1" extradata="1" />
//
// F1 2011 will then send UDP packets to the locahost on port 20777 providing real time
// telemetry.  Note that the socket service in C# will buffer up datagrams sent from the
// game which means that for real-time applications the code either needs to keep up
// or include logic to throw away old data.
//
// The data collection thread writes into a shared Struct variable which is protected
// by a Mutex.  The main thread samples this variable periodically, triggered by a timer
// and updates the telemetry displayed on the form.
//
// 
//

namespace F1_2011_Telemetry
{
    public partial class Form1 : Form
    {
        // Constants
        private const int PORTNUM = 20777;
        private const string IP = "127.0.0.1";
        private const int TIMERINTERVAL_MS = 100;        // refresh display every 10th of a sec
        
        // This is the IP endpoint we are connecting to (i.e. the IP and Port F1 2011 is sending to)
        private IPEndPoint remoteIP = new IPEndPoint(IPAddress.Parse(IP), PORTNUM);
        // This is the IP endpoint for capturing who actually sent the data
        private IPEndPoint senderIP = new IPEndPoint(IPAddress.Any, 0);
        // UDP Socket for the connection
        private UdpClient udpSocket;

        // Thread for capturing telemtry
        private Thread telemCaptureThread = null;
        
        // Holds the latest data captured from the game
        telemetryPacket latestData;
        telemetryPacket latestDataForDisplay;

        // Mutex used to protect latestData from simultaneous access by both threads
        static Mutex syncMutex = new Mutex();

        // Serial port for communicating with the Arduino
        private static SerialPort arduinoPort;

        // LED status for sending to the Arduino
        private byte arduinoLEDstatus = 0;

        // Constants for LED behaviour
        private static float REV_THRESHOLD = 1750;      // At what point 1st LED lights
        private static float REV_MAX = 1885;            // Max revs in game
        private static int NUM_LEDS = 8;                // Number of LEDs being controlled
        private static float LED_STEP = (REV_MAX - REV_THRESHOLD) / NUM_LEDS;
                                                        // Step (in Revs) from one LED to the next
        
        // Flags if the LEDs were at max last update - used to control the max revs flash
        private static bool maxLEDlastCycle = false;

        public Form1()
        {
            InitializeComponent();

            // Set up the socket for collecting game telemetry
            try
            {
                // Set up the socket to get F1 2011 telemetry
                udpSocket = new UdpClient();
                udpSocket.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
                udpSocket.ExclusiveAddressUse = false;
                udpSocket.Client.Bind(remoteIP);
                writeLog("Bound to socket on " + IP + ":" + PORTNUM.ToString());

                // Set up the serial port for comms with the Arduino board
                arduinoPort = new SerialPort();
                arduinoPort.PortName = "COM3";
                arduinoPort.BaudRate = 9600;
                arduinoPort.Open();
            }
            catch (Exception error)
            {
                writeLog(error.ToString());
                // throw;
            }
        }

        // Connect to the game.  This doesn't really connect (because UDP is a connectionless
        // protocol - it just kicks off the data collection thread to receive
        // the UDP packets and the timer for refreshing the screen display.
        private void Connect_Click(object sender, EventArgs e)
        {
            if (telemCaptureThread == null)
            {
                // Kick off a thread to start collecting data
                telemCaptureThread = new Thread(new ThreadStart(FetchData));
                telemCaptureThread.Start();
                writeLog("Starting capture...");

                // Turn off all the LEDs
                updateLEDs(0);

                // Kick off the timer to control screen updates
                timer1.Interval = TIMERINTERVAL_MS;
                timer1.Enabled = true;
                timer1.Start();
            }
        }

        // Disconnect from the game (stop the thread and timer)
        private void Disconnect_Click(object sender, EventArgs e)
        {
            if (telemCaptureThread != null)
            {
                telemCaptureThread.Abort();
                telemCaptureThread = null;
                writeLog("End capture.");
                timer1.Stop();
            }
        }
        
        private void Exit_Click(object sender, EventArgs e)
        {
            if (telemCaptureThread != null)
                telemCaptureThread.Abort();
            timer1.Stop();
            Application.Exit();
        }
        
        // Method that periodically (per invocations through the timer) updates
        // the displayed telemetry
        private void UpdateDisplay(Object myObject, EventArgs myEventArgs)
        {
            // Suspend the timer
            timer1.Stop();

            // Wait for mutex lock
            syncMutex.WaitOne();

            // Copy the data packet
            latestDataForDisplay = latestData;

            // Release the lock
            syncMutex.ReleaseMutex();

            // Update the display
            txtGear.Text = latestDataForDisplay.gear.ToString();
            txtEngineRate.Text = latestDataForDisplay.engineRate.ToString();
            txtThrottle.Text = latestDataForDisplay.throttle.ToString();
            txtLap.Text = latestDataForDisplay.lap.ToString();
            txtSpeed.Text = latestDataForDisplay.speed.ToString();
            txtLapTime.Text = latestDataForDisplay.lapTime.ToString();

            // Update the LEDs on the Arduino.
            float beyondThreshold = Math.Max(latestDataForDisplay.engineRate,REV_THRESHOLD) - REV_THRESHOLD;
            int numLEDsToLight = (int)(beyondThreshold / LED_STEP);
            byte shift = (byte)(numLEDsToLight > 8 ? 8 : numLEDsToLight);
            arduinoLEDstatus = (byte) (255 >> (8-shift));

            // Check for max revs and manipulate the LED status to
            // create alternate flashes
            if (latestDataForDisplay.gear != 0)
                if (arduinoLEDstatus == 255 && maxLEDlastCycle)
                {
                    maxLEDlastCycle = false;
                    arduinoLEDstatus = 0;
                }
                else
                {
                    maxLEDlastCycle = true;
                }
            updateLEDs(arduinoLEDstatus);

            txtLEDshift.Text = (8-shift).ToString();


            Application.DoEvents();

            // Restart the timer
            timer1.Start();

        }

        // Send the LED state to the Arduino
        private void updateLEDs(byte value)
        {
            // Send the byte value for the LEDs to the 
            // serial port attached to the Arduino
            arduinoPort.Write(new byte[] { value }, 0, 1);
        }

        // This method runs continously in the data collection thread.  It
        // waits to receive UDP packets from the game, converts them and writes
        // them to the shared struct variable.
        private void FetchData()
        {
            while (true)
            {
                // Get the data (this will block until we get a packet)
                Byte[] receiveBytes = udpSocket.Receive(ref senderIP);

                // Lock access to the shared struct
                syncMutex.WaitOne();

                // Convert the bytes received to the shared struct
                latestData = ConvertToPacket(receiveBytes);
                
                // Release the lock again
                syncMutex.ReleaseMutex();
            }
        }

        // Helper method to convert the bytes received from the UDP packet into the
        // struct variable format.
        telemetryPacket ConvertToPacket(byte[] bytes)
        {
            // Marshal the byte array into the telemetryPacket structure
            GCHandle handle = GCHandle.Alloc(bytes, GCHandleType.Pinned);
            telemetryPacket stuff = (telemetryPacket)Marshal.PtrToStructure(
                handle.AddrOfPinnedObject(), typeof(telemetryPacket));
            handle.Free();
            return stuff;
        }

        private void writeLog(string log)
        {
            txtOutput.Text += log + "\r\n";
        }
    }

    // This is the telemetry packet definition. My thanks to "korrea" for posting these
    // details on the CM forums
    struct telemetryPacket
    {
        public float time;
        public float lapTime;
        public float lapDistance;
        public float distance;
        public float x;
        public float y;
        public float z;
        public float speed;
        public float xv;
        public float yv;
        public float zv;
        public float xr;
        public float yr;
        public float zr;
        public float xd;
        public float yd;
        public float zd;
        public float susp_pos_bl;
        public float susp_pos_br;
        public float susp_pos_fl;
        public float susp_pos_fr;
        public float susp_vel_bl;
        public float susp_vel_br;
        public float susp_vel_fl;
        public float susp_vel_fr;
        public float wheel_speed_bl;
        public float wheel_speed_br;
        public float wheel_speed_fl;
        public float wheel_speed_fr;
        public float throttle;
        public float steer;
        public float brake;
        public float clutch;
        public float gear;
        public float gforce_lat;
        public float gforce_lon;
        public float lap;
        public float engineRate;
    }
}
